/**
 * @file webots_controller.h
 * @author JozenLee(2250017028@qq.com)
 * @brief this file is about webots controller
 * @version 0.1
 * @date 2022-03-26
 * 
 * @copyright Copyright (c) 2022
 * 
 */
#ifndef __WEBOTS_CONTROLLER__
#define __WEBOTS_CONTROLLER__

#include "ros/ros.h"
#include "common/RobotCommand.h"
#include "common/LegData.h"
#include "common/CheaterState.h"
#include "common/ImuData.h"
#include <webots/Robot.hpp>
#include <webots/Motor.hpp>
#include <webots/InertialUnit.hpp>
#include <webots/PositionSensor.hpp>
#include <webots/Supervisor.hpp>
#include <webots/Accelerometer.hpp>
#include <webots/TouchSensor.hpp>
#include <webots/Gyro.hpp>

using namespace webots;

class webotsController{
public:
    webotsController(ros::NodeHandle* n):_n(n){
        robot = new Supervisor();
        
        timeStep = (int)robot->getBasicTimeStep();
    }
    ~webotsController(){ delete robot;}
    void initNode(void);
    void initDevice(void);
    void run(void);
private:
    ros::NodeHandle* _n;
    ros::Publisher legdata_puber;
    ros::Publisher state_puber;
    ros::Publisher imu_puber;
    ros::Publisher touchstate_puber;
    ros::Subscriber cmd_suber;

    int timeStep;
    const float ang_bias = 0.523; 
    InertialUnit* imu;
    Accelerometer* accel;
    Gyro* gyro;
    Motor*  m[4][3];
    PositionSensor* p[4][3];
    TouchSensor* tc[4];
    Node* world_p_v_visor;
    Node* motor_v_visor[4][3];
    Supervisor* robot;

    common::LegData leg_data;
    common::CheaterState cheater_state;
    common::ImuData imu_data;

    void robotCmd_Callback(const common::RobotCommandConstPtr& msg);
    void updateLegData(void);
    void updateCheaterState(void);
    void updateTouchForce(void);
    void updateImuData(void);
};

#endif //__WEBOTS_CONTROLLER__